#include <iostream>
#include <string>
#include <sstream>
#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/videoio.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/calib3d.hpp> // 用于SGBM

#include <ros/ros.h>
#include <cv_bridge/cv_bridge.h>
// 同步订阅
// #include <image_transport/image_transport.h>
// #include <message_filters/subscriber.h>
// #include <message_filters/time_synchronizer.h>
// #include <message_filters/sync_policies/approximate_time.h>

using namespace std;
using namespace cv;

uchar HNY_CV_002 = 0;

#define Image_Width   1280
#define Image_height  720

Size imageSize = Size(Image_Width, Image_height);

Mat mapLx, mapLy, mapRx, mapRy;
Mat Rl, Rr, Pl, Pr, Q;
Mat xyz;

Rect validROIL;
Rect validROIR;

Mat rectifyImageL, rectifyImageR;


/*左目相机标定参数------------------------
fc_left_x   0            cc_left_x
0           fc_left_y    cc_left_y
0           0            1
-----------------------------------------*/

//左目相机矩阵
cv::Mat cameraMatrixL = (cv::Mat_<double>(3, 3) << 3276.85, 0, 689.805,
                                                            0, 3276.26, 380.782,
                                                            0, 0, 1);

//左目畸变系数
cv::Mat distCoeffL = (cv::Mat_<double>(5, 1) << -0.0271106, -3.07674, -0.00296706, 0.00423482, 161.223);
//[kc_left_01,  kc_left_02,  kc_left_03,  kc_left_04,   kc_left_05]


/*右目相机标定参数------------------------
fc_right_x   0              cc_right_x
0            fc_right_y     cc_right_y
0            0              1
-----------------------------------------*/
//右目相机矩阵
cv::Mat cameraMatrixR = (cv::Mat_<double>(3, 3) << 3280.18, 0, 640.318,
                                                            0, 3280.09, 390.449,
                                                            0, 0, 1);

//右目畸变系数
cv::Mat distCoeffR = (cv::Mat_<double>(5, 1) << -0.0596768, 2.03298, 0.000210255, 0.00183348, -53.8368);
//[kc_right_01,  kc_right_02,  kc_right_03,  kc_right_04,   kc_right_05]


cv::Mat T = (cv::Mat_<double>(3, 1) <<  -179.793, 0, 0);        //T平移向量
//[T_01,        T_02,       T_03]

cv::Mat R = (cv::Mat_<double>(3, 3) << 0.999938, 0.00220454, 0.0108998, -0.00216748, 0.999992, -0.00341085, -0.0109072, 0.00338702, 0.999935);   //rec旋转向量
//[rec_01,     rec_02,     rec_03]


const char* keys =
        {
                "{help h usage ? | | print this message}"
                "{@video | | Video file, if not defined try to use webcamera}"
        };

void GenerateFalseMap(cv::Mat &src, cv::Mat &disp)
{
    float max_val = 255.0f;
    float map[8][4] = { { 0,0,0,114 },{ 0,0,1,185 },{ 1,0,0,114 },{ 1,0,1,174 },
                        { 0,1,0,114 },{ 0,1,1,185 },{ 1,1,0,114 },{ 1,1,1,0 } };
    float sum = 0;
    for (int i = 0; i<8; i++)
        sum += map[i][3];

    float weights[8];
    float cumsum[8];
    cumsum[0] = 0;
    for (int i = 0; i<7; i++) {
        weights[i] = sum / map[i][3];
        cumsum[i + 1] = cumsum[i] + map[i][3] / sum;
    }

    int height_ = src.rows;
    int width_ = src.cols;

    for (int v = 0; v<height_; v++) {
        for (int u = 0; u<width_; u++) {

            float val = std::min(std::max(src.data[v*width_ + u] / max_val, 0.0f), 1.0f);

            int i;
            for (i = 0; i<7; i++)
                if (val<cumsum[i + 1])
                    break;

            float   w = 1.0 - (val - cumsum[i])*weights[i];
            uchar r = (uchar)((w*map[i][0] + (1.0 - w)*map[i + 1][0]) * 255.0);
            uchar g = (uchar)((w*map[i][1] + (1.0 - w)*map[i + 1][1]) * 255.0);
            uchar b = (uchar)((w*map[i][2] + (1.0 - w)*map[i + 1][2]) * 255.0);

            disp.data[v*width_ * 3 + 3 * u + 0] = b;                       //rgbÄÚ´æÁ¬Ðø´æ·Å
            disp.data[v*width_ * 3 + 3 * u + 1] = g;
            disp.data[v*width_ * 3 + 3 * u + 2] = r;
        }
    }
}


int main(int argc, const char** argv)
{
    CommandLineParser parser(argc, argv, keys);
    parser.about("Video Capture");

    if (parser.has("help"))
    {
        parser.printMessage();
        return 0;
    }
    String videoFile = parser.get<String>(0);

    if (!parser.check())
    {
        parser.printErrors();
        return 0;
    }

    VideoCapture cap;
    if (videoFile != "")
    {
        cap.open(videoFile);
    }
    else
    {
        cap.open(0);

        cap.set(cv::CAP_PROP_FRAME_WIDTH, Image_Width * 2);
        cap.set(cv::CAP_PROP_FRAME_HEIGHT, Image_height);
    }
    if (!cap.isOpened())
    {
        cout << "Cannot open the video file" << endl;
        return -1;
    }

    // 获取摄像头的帧率
    double fps = cap.get(cv::CAP_PROP_FPS);

    Mat frame, frame_L, frame_R;

    cap >> frame;

    double fScale = 0.5;
    Size dsize = Size(Image_Width / 2, Image_height / 2);
    Mat image_L_resize = Mat(dsize, CV_32S);
    Mat image_R_resize = Mat(dsize, CV_32S);
    char file_name[100];
    uchar num = 1;

    uchar Resize_Flag = 0;
    int key = 0;

    Size videoSize(Image_Width * 2, Image_height);
    VideoWriter writer("VideoTest.avi", cv::VideoWriter::fourcc('M', 'J', 'P', 'G'), fps, videoSize);

    // 创建SGBM对象
    Ptr<StereoSGBM> sgbm = StereoSGBM::create(0, 128, 3);
    sgbm->setP1(100);
    sgbm->setP2(1000);
    sgbm->setMinDisparity(0);
    sgbm->setNumDisparities(128);
    sgbm->setBlockSize(3);
    sgbm->setPreFilterCap(63);
    sgbm->setUniquenessRatio(8);
    sgbm->setSpeckleWindowSize(200);
    sgbm->setSpeckleRange(32);
    sgbm->setDisp12MaxDiff(1);

    stereoRectify(cameraMatrixL, distCoeffL, cameraMatrixR, distCoeffR, imageSize, R, T, Rl, Rr, Pl, Pr, Q, CALIB_ZERO_DISPARITY,
                  0, imageSize, &validROIL, &validROIR);
    initUndistortRectifyMap(cameraMatrixL, distCoeffL, Rl, Pl, imageSize, CV_32FC1, mapLx, mapLy);
    initUndistortRectifyMap(cameraMatrixR, distCoeffR, Rr, Pr, imageSize, CV_32FC1, mapRx, mapRy);

    while (true)
    {
        key = waitKey(1);

        cap >> frame;

        if (HNY_CV_002 == 0)
        {
            frame_L = frame(Rect(0, 0, Image_Width, Image_height));
            frame_R = frame(Rect(Image_Width, 0, Image_Width, Image_height));
        }
        else
        {
            frame_L = frame(Rect(Image_Width, 0, Image_Width, Image_height));
            frame_R = frame(Rect(0, 0, Image_Width, Image_height));
        }

        if (key == 'R')
        {
            Resize_Flag = !Resize_Flag;
            key = 0;
        }
        if (Resize_Flag == 1)
        {
            resize(frame_L, image_L_resize, dsize);
            namedWindow("Video_L", 1);
            imshow("Video_L", image_L_resize);
            resize(frame_R, image_R_resize, dsize);
            namedWindow("Video_R", 1);
            imshow("Video_R", image_R_resize);
        }
        else
        {
            namedWindow("Video_L", 1);
            imshow("Video_L", frame_L);
            namedWindow("Video_R", 1);
            imshow("Video_R", frame_R);
        }

        // 计算视差图
        Mat gray_L, gray_R;
        cvtColor(frame_L, gray_L, COLOR_BGR2GRAY);
        cvtColor(frame_R, gray_R, COLOR_BGR2GRAY);

        remap(gray_L, rectifyImageL, mapLx, mapLy, INTER_LINEAR);
        remap(gray_R, rectifyImageR, mapRx, mapRy, INTER_LINEAR);

        Mat disp, disp8;
        sgbm->compute(rectifyImageL, rectifyImageR, disp);
        Mat disp8U = Mat(disp.rows, disp.cols, CV_8UC1);

        reprojectImageTo3D(disp, xyz, Q, true);
        xyz = xyz * 16;

        disp.convertTo(disp, CV_32F, 1.0 / 16);
        normalize(disp, disp8U, 0, 255, NORM_MINMAX, CV_8UC1);

        medianBlur(disp8U, disp8U, 9);

        Mat dispcolor(disp8U.size(), CV_8UC3);
        GenerateFalseMap(disp8U, dispcolor);
        imshow("disparity", dispcolor);

        if (key == 'C')
        {
            key = 0;
            while (key != 'C')
            {
                cap >> frame;
                writer << frame;
                key = waitKey(1);
            }
            key = 0;
        }

        if (key == 'q')
        {
            break;
        }
    }

    cap.release();
    writer.release();
    destroyAllWindows();
    return 0;
}
